/**
 * @file lidarFactor.hpp
 * @author Linfu Wei (ghowoght@qq.com)
 * @brief 用于ceres非线性优化的仿函数
 * @version 1.0
 * @date 2022-06-05
 * 
 * @copyright Copyright (c) 2022
 * 
 */
#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <Eigen/Dense>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl_conversions/pcl_conversions.h>

struct LidarEdgeFactor{
    Eigen::Vector3d curr_point, last_point_a, last_point_b;
    Eigen::Vector3d t_w_last_;
    Eigen::Quaterniond q_w_last_;

    double s;
    LidarEdgeFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_,
                    Eigen::Vector3d last_point_b_, double s_,
                    Eigen::Quaterniond q_w_last_,
                    Eigen::Vector3d t_w_last_)
        : curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_), s(s_)
        , q_w_last_(q_w_last_), t_w_last_(t_w_last_){ }

    template<typename T>
    bool operator()(const T* q, const T* t, T* residual) const{
        Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
        Eigen::Matrix<T, 3, 1> lpa{T(last_point_a.x()), T(last_point_a.y()), T(last_point_a.z())};
        Eigen::Matrix<T, 3, 1> lpb{T(last_point_b.x()), T(last_point_b.y()), T(last_point_b.z())};

        Eigen::Quaternion<T> q_w_last{(T)q_w_last_.w(), (T)q_w_last_.x(), (T)q_w_last_.y(), (T)q_w_last_.z()};
        Eigen::Matrix<T, 3, 1> t_w_last{(T)t_w_last_(0), (T)t_w_last_(1), (T)t_w_last_(2)};

        Eigen::Quaternion<T> q_w_increm{q[3], q[0], q[1], q[2]};
        // Eigen::Matrix<T, 3, 1> t_w_increm{t[0], t[1], t[2]};
        Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};
        
        // 运动补偿
        q_w_increm = q_identity.slerp(T(s), q_w_increm);
        Eigen::Matrix<T, 3, 1> t_w_increm{T(s) * t[0], T(s) * t[1], T(s) * t[2]};

        Eigen::Matrix<T, 3, 1> wp;
        wp = q_w_last * (q_w_increm * cp + t_w_increm) + t_w_last; // 将curr_point变换到世界坐标系/上一帧的坐标系
        
        // 计算点到线的距离 （OA x OB） / |AB|
        Eigen::Matrix<T, 3, 1> nu = (wp - lpa).cross(wp - lpb);
        Eigen::Matrix<T, 3, 1> de = lpa - lpb;
        // residual[0] = nu.x() / de.norm();
        // residual[1] = nu.y() / de.norm();
        // residual[2] = nu.z() / de.norm();

        residual[0] = nu.norm() / de.norm();

        return true;
    }

    static ceres::CostFunction* Create( const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_a_,
                                        const Eigen::Vector3d last_point_b_, const double s_, 
                                        Eigen::Quaterniond q_w_last_,
                                        Eigen::Vector3d t_w_last_)
    {
        // 残差维数，q维数，t维数
        return new ceres::AutoDiffCostFunction<LidarEdgeFactor, 1, 4, 3>(
            new LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, s_, q_w_last_, t_w_last_)
        );
    }
};

struct LidarSurfFactor{
    Eigen::Vector3d curr_point, last_point_j, last_point_l, last_point_m;
    Eigen::Vector3d ljm_norm; // jlm平面归一化法向量
    Eigen::Vector3d t_w_last_;
    Eigen::Quaterniond q_w_last_;
    double s;
    LidarSurfFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_j_,
                    Eigen::Vector3d last_point_l_, Eigen::Vector3d last_point_m_, double s_,
                    Eigen::Quaterniond q_w_last_,
                    Eigen::Vector3d t_w_last_)
        : curr_point(curr_point_), last_point_j(last_point_j_), last_point_l(last_point_l_), last_point_m(last_point_m_), s(s_)
        , q_w_last_(q_w_last_), t_w_last_(t_w_last_)
    {
        // 计算jlm法向量
        ljm_norm = (last_point_j - last_point_l).cross(last_point_j - last_point_m);
        ljm_norm.normalize();
    }

    template<typename T>
    bool operator()(const T* q, const T* t, T* residual) const{
        Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
        Eigen::Matrix<T, 3, 1> lpj{T(last_point_j.x()), T(last_point_j.y()), T(last_point_j.z())};
        Eigen::Matrix<T, 3, 1> ljm{T(ljm_norm.x()), T(ljm_norm.y()), T(ljm_norm.z())};

        Eigen::Quaternion<T> q_w_last{(T)q_w_last_.w(), (T)q_w_last_.x(), (T)q_w_last_.y(), (T)q_w_last_.z()};
        Eigen::Matrix<T, 3, 1> t_w_last{(T)t_w_last_(0), (T)t_w_last_(1), (T)t_w_last_(2)};

        Eigen::Quaternion<T> q_w_increm{q[3], q[0], q[1], q[2]};
        // Eigen::Matrix<T, 3, 1> t_w_increm{t[0], t[1], t[2]};
        
        Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};
        
        // 运动补偿
        q_w_increm = q_identity.slerp(T(s), q_w_increm);
        Eigen::Matrix<T, 3, 1> t_w_increm{T(s) * t[0], T(s) * t[1], T(s) * t[2]};

        Eigen::Matrix<T, 3, 1> wp;
        wp = q_w_last * (q_w_increm * cp + t_w_increm) + t_w_last; // 将curr_point变换到世界坐标系/上一帧的坐标系
        
        // 计算点到平面的距离
        residual[0] = (wp - lpj).dot(ljm);

        return true;
    }

    static ceres::CostFunction* Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_j_,
                    const Eigen::Vector3d last_point_l_, const Eigen::Vector3d last_point_m_, const double s_,
                    Eigen::Quaterniond q_w_last_,
                    Eigen::Vector3d t_w_last_)
    {
        return new ceres::AutoDiffCostFunction<LidarSurfFactor, 1, 4, 3>(
            new LidarSurfFactor(curr_point_, last_point_j_, last_point_l_, last_point_m_, s_, q_w_last_, t_w_last_)
        );
    }
};